Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Refactor attitude control to be thread safe #27839

Merged
merged 15 commits into from
Sep 26, 2024

Conversation

andyp1per
Copy link
Collaborator

@andyp1per andyp1per commented Aug 14, 2024

Broken out from #27029

The essence of these changes is to make sure that the interaction between the attitude controller and the rate controller are thread safe. This involves avoiding setting object state as intermediate values but instead publishing at the end of a calculation. It also means that changes should be accepted as vectors rather than discrete axes. Experiments were also done using locking but this proved to be moderately expensive at high update rates and did not improve the overall effect of this change.

@tpwrules
Copy link
Contributor

Does this just assume that a Vector3f set is atomic?

@andyp1per
Copy link
Collaborator Author

Does this just assume that a Vector3f set is atomic?

It assumes that it doesn't matter that its not. The individual axes are and because all of the output is sequential, even though you might get some object tearing in practice it doesn't affect things. What does matter is setting things to 0 or other values in intermediate steps - that does make a big difference.

Copy link
Member

@IamPete1 IamPete1 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Plane does not call these functions in the same order as copter in a loop. I suspect plane would now be using the gyro from one loop behind?

It also makes me slightly nervous that this is all a bit subtle so we could easily break it in the future.

I wonder if a little structure "rate controller inputs" or something like that would make it a bit clearer. You could also have a sempahore on changing those values, it may be a bit of performance cost but it does at least act as a signal that the developer needs to be careful.

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp Outdated Show resolved Hide resolved
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp Outdated Show resolved Hide resolved
@andyp1per
Copy link
Collaborator Author

I wonder if a little structure "rate controller inputs" or something like that would make it a bit clearer. You could also have a sempahore on changing those values, it may be a bit of performance cost but it does at least act as a signal that the developer needs to be careful.

Perhaps simply using a naming prefix would be sufficient here - I have always found it confusing that we have both attitude control and rate control mixed up in the same class

@tpwrules
Copy link
Contributor

Does this just assume that a Vector3f set is atomic?

It assumes that it doesn't matter that its not. The individual axes are and because all of the output is sequential, even though you might get some object tearing in practice it doesn't affect things. What does matter is setting things to 0 or other values in intermediate steps - that does make a big difference.

I'm concerned about this assumption on non-microcontrollers, the tear could be pretty big. In any case, wouldn't it break replay?

@andyp1per
Copy link
Collaborator Author

I'm concerned about this assumption on non-microcontrollers, the tear could be pretty big. In any case, wouldn't it break replay?

Tearing will only happen when the rate loop is in a separate thread. With replay all bets are off with a separate thread - I don't see how this can be made to work well regardless of the locking strategy.

@andyp1per andyp1per force-pushed the pr-att-dt branch 2 times, most recently from 4ed781d to 2b73c13 Compare September 4, 2024 17:40
Copy link
Contributor

@lthall lthall left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is looking good. A few PR's to get in first will simplify it further.

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp Outdated Show resolved Hide resolved
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp Outdated Show resolved Hide resolved
libraries/AC_AutoTune/AC_AutoTune_Multi.cpp Show resolved Hide resolved
libraries/AC_AutoTune/AC_AutoTune_Multi.cpp Show resolved Hide resolved
libraries/AC_AutoTune/AC_AutoTune_Multi.cpp Show resolved Hide resolved
libraries/AP_AHRS/AP_AHRS_Logging.cpp Outdated Show resolved Hide resolved
Copy link
Member

@IamPete1 IamPete1 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think this all looks good for copter and plane. Just needs some some fixups for sub and heli.

Sub needs the new rate_controller_target_reset call after its rate_controller_run.

The sub classes AC_AttitudeControl_Heli, AC_AttitudeControl_Multi_6DoF and AC_AttitudeControl_Sub all override rate_controller_run I think they need to be changed to override the new rate_controller_run_dt.

The heli method passthrough_bf_roll_pitch_rate_yaw is quite tangled, it has not been made safe, I think that is OK, we just need to add a big comment so we can do it at some future point. Likewise for the functions in AC_AttitudeControl_TS

@andyp1per
Copy link
Collaborator Author

I think this all looks good for copter and plane. Just needs some some fixups for sub and heli.

Sub needs the new rate_controller_target_reset call after its rate_controller_run.

The sub classes AC_AttitudeControl_Heli, AC_AttitudeControl_Multi_6DoF and AC_AttitudeControl_Sub all override rate_controller_run I think they need to be changed to override the new rate_controller_run_dt.

The heli method passthrough_bf_roll_pitch_rate_yaw is quite tangled, it has not been made safe, I think that is OK, we just need to add a big comment so we can do it at some future point. Likewise for the functions in AC_AttitudeControl_TS

I believe that all the subclasses that override rate_controller_run() are correct. None of them support sysid or pd_scale from the base class (which is what rate_controller_target_reset does) and so can be left as-is for now.

I agree that Heli could be refactored to use that function, but was planning to leave it alone for now since @bnsgeyer doesn't want fast rate support in Heli for now.

rate_controller_run_dt is currently the equivalent of what was rate_controller_run() which was only used by copter and plane and only exists in AC_AttitudeControl_Multi so cannot be overridden.

So I agree but that some further refactoring could be done but propose that we leave it as-is in this PR to avoid changing the behaviour of these other vehicles.

@andyp1per
Copy link
Collaborator Author

@IamPete1 I have added a config error to the default implementation of rate_controller_run_dt() to avoid it getting called accidentally

ArduCopter/Attitude.cpp Outdated Show resolved Hide resolved
@tridge
Copy link
Contributor

tridge commented Sep 23, 2024

needs testing on quadplane, either real aircraft or realflight
especially need to check the scale factor applied to rate control in transition

@rmackay9
Copy link
Contributor

This looks OK to me

I agree that we should be sure to test Heli and Sub to be sure they're unaffected and of course, we should test with the mindset that there is a problem and we just haven't found it yet rather than just to prove that it works as expected.

@tridge
Copy link
Contributor

tridge commented Sep 24, 2024

I've tested quadplane in RealFlight and it does look good to me

@andyp1per
Copy link
Collaborator Author

We have done manual testing of the gain backoff on landing and takeoff successfully and I have also written an autotest to verify which passes - #28220

@Williangalvani
Copy link
Contributor

couldn't find any issues running this on Sub

@peterbarker
Copy link
Contributor

Just waiting on @bnsgeyer to be happy with this and then we can merge it!

@MattKear
Copy link
Contributor

I will have some time tomorrow to test this on heli in Real Flight, if @bnsgeyer doesn't have chance before then.

@andyp1per
Copy link
Collaborator Author

Thanks @MattKear !

@bnsgeyer
Copy link
Contributor

@andyp1per @rmackay9 I tested heli in real flight. It looks fine. No issues.

@peterbarker peterbarker merged commit 4c1c326 into ArduPilot:master Sep 26, 2024
95 checks passed
@andyp1per andyp1per deleted the pr-att-dt branch September 26, 2024 09:27
@andyp1per
Copy link
Collaborator Author

Thanks all!

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.